import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

plt.rcParams['font.sans-serif'] = ['Microsoft YaHei']
plt.rcParams['axes.unicode_minus'] = False
plt.switch_backend('TkAgg')

target_true = np.array([0, 200, 0])  # 真目标
r,h = 7,10
target_fake = np.array([0, 0, 0])  # 假目标

missile_M1 = np.array([20000, 0, 2000]) # M1初始位置和速度
missile_speed = 300
drone_FY1 = np.array([17800, 0, 1800]) # FY1初始位置和速度
drone_speed = 120

g = 9.8 #重力加速度
smoke_radius = 10  # 烟幕有效遮蔽半径10m（云团中心10m内有效）
smoke_sink_speed = 3  # 烟幕匀速下沉速度3m/s
smoke_valid_time = 20  # 烟幕起爆后20s内有效
drone_drop_delay = 1.5  # FY1受领任务后1.5s投放烟幕
smoke_detonate_delay = 3.6  # 烟幕投放后3.6s起爆

#-------------------------- 假设真目标被长方体包着 --------------------------
def get_cylinder_bounding_box_vertices(target_center, radius, height):
    # 1. 确定长方体的边界范围（基于圆柱体的外切条件）
    # 长方体x轴范围：圆柱体底面圆心x±半径（左右方向相切）
    x_min = target_center[0] - radius
    x_max = target_center[0] + radius
    # 长方体y轴范围：圆柱体底面圆心y±半径（前后方向相切）
    y_min = target_center[1] - radius
    y_max = target_center[1] + radius
    # 长方体z轴范围：圆柱体下底面z（0）到上底面z（高度）（上下底面贴合）
    z_min = target_center[2]
    z_max = target_center[2] + height

    # 2. 生成8个顶点坐标（按固定顺序排列，便于后续可视化或计算）
    vertices = [
        # 下底面4个顶点（z=z_min）：前下左、前下右、后下右、后下左
        np.array([x_min, y_min, z_min]),
        np.array([x_max, y_min, z_min]),
        np.array([x_max, y_max, z_min]),
        np.array([x_min, y_max, z_min]),
        # 上底面4个顶点（z=z_max）：前上左、前上右、后上右、后上左
        np.array([x_min, y_min, z_max]),
        np.array([x_max, y_min, z_max]),
        np.array([x_max, y_max, z_max]),
        np.array([x_min, y_max, z_max])
    ]
    return vertices


# -------------------------- 2. 实体运动计算 --------------------------
#计算导弹位置
def missile_real_time_position(missile_init_pos, t):
    if t < 0: #解决小于0的异常
        return missile_init_pos

    # 计算导弹飞行方向单位向量（从初始位置指向假目标）
    dir_vec = target_fake - missile_init_pos
    dir_vec_magnitude = np.linalg.norm(dir_vec)
    dir_unit_vec = dir_vec / dir_vec_magnitude if dir_vec_magnitude > 1e-6 else np.array([0, 0, 0])

    # 4. 实时位置计算
    displacement = missile_speed * t * dir_unit_vec
    missile_current_pos = missile_init_pos + displacement

    return missile_current_pos

#计算烟雾弹中心点位置
def get_smoke_explosion_coords(drone_init_pos, drone_speed, t):
    EXPLOSION_T = drone_drop_delay + smoke_detonate_delay  # 爆炸时刻=投放延迟+起爆延迟=5.1s
    TIME_TOL = 1e-6  # 时间误差容限

    # 非爆炸时刻返回(0.0, 0.0, 0.0)
    if not (EXPLOSION_T - TIME_TOL <= t <= EXPLOSION_T + TIME_TOL):
        return np.array([0.0, 0.0, 0.0])

    # 1. 计算下爆炸前的下x和y轴
    dir_vec = target_fake - drone_init_pos
    # 单位方向向量（仅表示方向，消除距离影响）
    dir_norm = np.linalg.norm(dir_vec)
    dir_unit = dir_vec / dir_norm if dir_norm > 1e-6 else np.array([0, 0, 0])
    total_horizontal_displacement = drone_speed * EXPLOSION_T * dir_unit
    explosion_xy = drone_init_pos[:2] + total_horizontal_displacement[:2]

    # 2. 烟幕自由下落后的z坐标
    drop_z = drone_init_pos[2]
    t = t - drone_drop_delay
    fall_distance = 0.5 * g * (t ** 2)
    explosion_z = max(drop_z - fall_distance, 0.0)

    # 3. 爆炸时刻最终坐标
    explosion_coords = np.array([round(explosion_xy[0], 1),round(explosion_xy[1], 1),round(explosion_z, 1)])

    return explosion_coords


# -------------------------- 3. 遮蔽判断 --------------------------
# -------------------------- 判断函数1：“导弹-顶点视线线段”与“云团球体”是否相交 --------------------------
def is_segment_sphere_intersect(missile_pos, vertex_pos, smoke_center, smoke_radius):
    """
    判断导弹到长方体顶点的视线线段（AB）是否与云团有效球体（球心C，半径r）相交
    参数：
        missile_pos: 实时导弹位置（np.array，[x,y,z]）
        vertex_pos: 长方体顶点位置（np.array，[x,y,z]）
        smoke_center: 实时烟幕云团中心（np.array，[x,y,z]）
        smoke_radius: 烟幕有效半径（文档给定10m，）
    返回：
        bool: 相交返回True，不相交返回False
    """
    # 计算核心向量
    vec_AC = smoke_center - missile_pos  # 向量：导弹→云团中心
    vec_AB = vertex_pos - missile_pos    # 向量：导弹→长方体顶点（视线方向）

    # 计算AB的长度平方（避免直接开方，提升效率并规避数值误差）
    ab_sq_mag = np.dot(vec_AB, vec_AB)
    # 若AB长度趋近于0（导弹与顶点几乎重合，实际场景不可能），返回不相交
    if ab_sq_mag < 1e-12:
        return False

    # 计算参数k（定位线段上离云团中心最近的点，）
    k = np.dot(vec_AC, vec_AB) / ab_sq_mag

    # 分情况计算“云团中心到线段AB的最短距离”
    if k <= 0.0:
        # 情况1：最近点为线段起点A（导弹位置），距离=|AC|
        min_dist = np.linalg.norm(vec_AC)
    elif 0.0 < k < 1.0:
        # 情况2：最近点在线段AB内部，距离=|AC×AB| / |AB|（叉积模长/AB长度）
        cross_prod = np.cross(vec_AC, vec_AB)
        min_dist = np.linalg.norm(cross_prod) / np.sqrt(ab_sq_mag)
    else:
        # 情况3：最近点为线段终点B（顶点位置），距离=|BC|（向量BC=云团中心→顶点）
        vec_BC = smoke_center - vertex_pos
        min_dist = np.linalg.norm(vec_BC)

    # 若最短距离≤烟幕半径，线段与球体相交（视线被云团遮挡）
    return min_dist <= smoke_radius + 1e-6  # 加1e-6规避浮点精度误差


# -------------------------- 判断函数2：综合判断t时刻是否实现有效遮蔽 --------------------------
def is_effective_occlusion(t):
    """
    综合判断t时刻烟幕是否对真目标实现有效遮蔽（基于文档规则，）
    参数：
        t: 当前时刻（s，从受领任务时刻开始计时，t≥0）
    返回：
        bool: 有效遮蔽返回True，无效返回False
    """
    # 1. 先判断时间有效性：烟幕仅在“起爆后20s内”有效
    detonate_t = drone_drop_delay + smoke_detonate_delay  # 烟幕起爆时刻（固定5.1s）
    smoke_start_t = detonate_t          # 有效遮蔽开始时间
    smoke_end_t = detonate_t + smoke_valid_time  # 有效遮蔽结束时间（5.1+20=25.1s）
    # 时间异常或超出有效窗口：无效遮蔽
    if t < 0.0 or t < smoke_start_t - 1e-6 or t > smoke_end_t + 1e-6:
        return False

    # 2. 计算t时刻的核心实体位置
    # 2.1 导弹实时位置
    missile_pos = missile_real_time_position(missile_M1, t)
    print("导弹实时位置",missile_pos)
    # 2.2 真目标外接长方体顶点（8个）
    target_vertices = get_cylinder_bounding_box_vertices(target_true, r, h)
    print("真目标外接长方体顶点（8个）",target_vertices)
    # 2.3 烟幕云团实时中心（起爆后匀速下沉，）
    # 先获取起爆时刻的云团中心（基础位置）
    explosion_center = get_smoke_explosion_coords(drone_FY1, drone_speed, detonate_t)
    print("起爆时刻的云团中心（基础位置）",explosion_center)
    # 计算t时刻的下沉距离：smoke_sink_speed*(t - detonate_t)
    sink_distance = smoke_sink_speed * (t - detonate_t)
    # 实时云团中心（x、y不变，z轴下沉，最低到地面z=0）
    smoke_center_z = max(explosion_center[2] - sink_distance, 0.0)
    smoke_center = np.array([explosion_center[0], explosion_center[1], smoke_center_z])

    # 3. 遍历长方体所有顶点：只要1个顶点的视线被遮挡，即视为有效遮蔽
    for vertex in target_vertices:
        if is_segment_sphere_intersect(missile_pos, vertex, smoke_center, smoke_radius):
            return True  # 有一个顶点视线被挡，即有效

    # 所有顶点视线均未被挡：无效遮蔽
    return False

# -------------------------- 4. 有效遮蔽时长计算 --------------------------


# -------------------------- 执行与结果输出 --------------------------
if __name__ == "__main__":
    # 1. 输出长方体顶点
    target_vertices = get_cylinder_bounding_box_vertices(target_true, r, h)
    vertex_names = ["前下左", "前下右", "后下右", "后下左", "前上左", "前上右", "后上右", "后上左"]
    print("真目标外切长方体8顶点坐标")
    for name, v in zip(vertex_names, target_vertices):
        print(f"{name}: ({v[0]}, {v[1]}, {v[2]}) m")


    print(f"有效遮蔽时长：{1} 秒")  # 修正后应为3~5秒（符合A题合理范围）
    print(is_effective_occlusion(5.2))

